
package org.nViron.Position;

import instar.dg.Positioning.*;
import instar.dg.Common.Orientation;
import instar.dg.Debug.Debug;

import org.freelocapi.location.Criteria;
import org.freelocapi.location.Location;
import org.freelocapi.location.LocationException;
import org.freelocapi.location.LocationListener;
import org.freelocapi.location.LocationProvider;

/**
 * @author Stefan
 */
public class FreeLocationAPI extends instar.dg.Positioning.PositionProvider implements LocationListener {

    public static final String INTERNAL_COMPASS = "INTERNAL_COMPASS";
    public static final String MINIMAL_SPEED_FOR_COURSE = "MINIMAL_SPEED_FOR_COURSE";

    private LocationProvider provider = null;
    public final static int DEFAULT_MINIMUM_SPEED_FOR_COURSE = 4;
    private int minimalSpeedForCourse = DEFAULT_MINIMUM_SPEED_FOR_COURSE;
    private boolean useInternalCompass = false;

    /**
     * Creates a new instance of FreeLocationAPI
     */
    public FreeLocationAPI() {
        interpolate = true;
    }

    @Override
    public void start() {
        Criteria crit1 = new Criteria();
//        crit1.setHorizontalAccuracy(25); // 25m
//        crit1.setVerticalAccuracy(25); // 25m
//        crit1.setCostAllowed(false);
        crit1.setSpeedAndCourseRequired(true);
        try {
            provider = LocationProvider.getInstance(crit1);
            if (provider != null) {
                //#debug
                Debug.out.println("provider '" + provider.getClass().getName() + "' found in state '" + provider.getState() + "'!");
                setState(provider.getState());
                // provider found!
                // .. see the complete source code for the real action.
                provider.setLocationListener(this, 1, -1, -1);
            } else {
                invokeFailure("Location API: no provider found!!");
                setState(OUT_OF_SERVICE);
                //#debug
                Debug.out.println("Location API: no provider found!!");
            }
        } catch (LocationException ex) {
            //#debug warn
            Debug.out.println("startLocationAPI: " + ex.toString());
        } catch (Exception e) {
            //#debug fatal
            Debug.out.println("startLocationAPI: " + e.toString());
        }
    }

    @Override
    public void stop() {
        super.stop();
        if ( provider != null ) {
            provider.setLocationListener(null, -1, -1, -1);
            provider.reset();
        }
    }

    @Override
    public void setAttribute(String key, Object value) {
        if ( INTERNAL_COMPASS.equals(key) ) {
            setUseInternalCompass( "true".equals(value));
        }
        if ( MINIMAL_SPEED_FOR_COURSE.equals(key)) {
            setMinimalSpeedForCourse( Integer.parseInt((String)value));
        }
        super.setAttribute(key, value);
    }

    public int getMinimalSpeedForCourse() {
        return minimalSpeedForCourse;
    }

    public void setMinimalSpeedForCourse(int minimalSpeedForCourse) {
        this.minimalSpeedForCourse = minimalSpeedForCourse;
    }

    public boolean isUseInternalCompass() {
        return useInternalCompass;
    }

    public void setUseInternalCompass(boolean useInternalCompass) {
        this.useInternalCompass = useInternalCompass;
    }

    private Orientation orient = new Orientation(0.0F, 0.0F, 0.0F);

    /**
     * @param provider
     * @param location
     */
    @Override
    public void locationUpdated(LocationProvider provider, Location location) {
        try {
            if ((location != null) && location.isValid() && (location.getQualifiedCoordinates() != null) && (location.getQualifiedCoordinates().getLongitude() != 0.0) && (location.getQualifiedCoordinates().getLatitude() != 0.0)) {
                //#debug
                Debug.out.println(System.currentTimeMillis() + " ms - [" + Thread.currentThread().getName() + "] locationUpdated: " + location.getQualifiedCoordinates().getLongitude() + "/" + location.getQualifiedCoordinates().getLatitude());
                instar.dg.Positioning.GPSPosition pos = new instar.dg.Positioning.GPSPosition(location.getQualifiedCoordinates().getLongitude(), location.getQualifiedCoordinates().getLatitude(), location.getQualifiedCoordinates().getAltitude());
                pos.setSpeed(location.getSpeed() * 3.6F); // convert m/s to km/h
//                Debug.out.println( "  Speed = " + pos.getSpeed() + " km/h  Course = " + location.getCourse() );
                if ( useInternalCompass) {
                    org.freelocapi.location.Orientation compassOrientation = null;
                    try {
                        compassOrientation = org.freelocapi.location.Orientation.getOrientation();
                    } catch (LocationException le) {
                        // do nothing
                        compassOrientation = null;
                    }
                    if (compassOrientation == null) {
                        if ((pos.getSpeed() > minimalSpeedForCourse) && (location.getCourse() != Float.NaN)) {
                            pos.setCourse(location.getCourse());
                            orient.setHeading(location.getCourse());
                        }
                    } else {
                        pos.setCourse(compassOrientation.getCompassAzimuth());
                    }
                } else {
                    if ((pos.getSpeed() > minimalSpeedForCourse) && (location.getCourse() != Float.NaN)) {
                        pos.setCourse(location.getCourse());
                        orient.setHeading(location.getCourse());
                    }
                }
                pos.setTimestamp(location.getTimestamp());
                pos.setHorizontalAccuracy(location.getQualifiedCoordinates().getHorizontalAccuracy());
                pos.setVerticalAccuracy(location.getQualifiedCoordinates().getVerticalAccuracy());
                parseNMEA(pos, location.getExtraInfo("application/X-jsr179-location-nmea"));
                interpolate(pos, orient);
            }
        } catch (Throwable t) {
            Debug.out.println(System.currentTimeMillis() + " ms - [" + Thread.currentThread().getName() + "] locationUpdated throws Exception: " + t.toString());
            t.printStackTrace();
        }
    }

    /**
     * @param position
     * @param nmea
     */
    protected final void parseNMEA(instar.dg.Positioning.GPSPosition position, String nmea) {
        //#debug
//        Debug.out.println( "nmea = '" + nmea + "'" );
        if (nmea == null) {
            return;
        }

        int begin_idx = nmea.indexOf('$');
        int end_idx = -1;
        if (begin_idx > -1) {
            end_idx = nmea.indexOf("$", begin_idx + 1) - 1;
        }
//        Debug.out.println(System.currentTimeMillis() + " ms GPS: while " + begin_idx + " / " + end_idx + " - " );
        while ((begin_idx > -1) && (end_idx > -1)) {
            String sLine = nmea.substring(begin_idx, end_idx).trim();
            String DATA_STRING = sLine.substring(6, sLine.length() - 3);
//            Debug.out.println( System.currentTimeMillis() + " ms GPS: sLine = '" + sLine + "'" );
            // Check the gps data type and convert the information to a more readable format.
            if (sLine.startsWith("$GPGGA")) {
                try {
//                    //#debug info
//                    Debug.out.println( System.currentTimeMillis() + " ms - FreeLocationAPI->parseNMEA(): $GPGGA found." );
//                    Debug.out.println(System.currentTimeMillis() + " ms splitString()");
                    String[] data = splitString(DATA_STRING);
                    // number of satellites must be greater than 3
//                                if ( Integer.valueOf( data[6] ).intValue() > 3 ) {
                    // check if GPS fix is available
                    if (Integer.valueOf(data[5]).intValue() > 0) {
//                                    System.out.println(System.currentTimeMillis() + " ms GPS: valid Position found!");
//                                    System.out.println( "long: " + data[3] + " - lat: " + data[1] );
/*                        String latD = data[1].substring( 0, 2 );
                        double lat = Integer.valueOf( latD ).floatValue() + Double.valueOf( data[1].substring( 2 ) ).doubleValue() / 60;
                        String lonD = data[3].substring( 0, 3 );
                        double lon = Integer.valueOf( lonD ).floatValue() + Double.valueOf( data[3].substring( 3 ) ).doubleValue() / 60;
                        //float alt = Float.valueOf( data[8] ).floatValue();
                        //                                  System.out.println( "long: " + lon + " - lat: " + lat );
                        // check if latitude is N or S
                        if ( data[2] == "S" )
                        lat *= -1;
                        // check if east or west
                        if ( data[4] == "W" )
                        lon *= -1;*/
                        if (!data[6].equals("")) {
                            byte sats = (byte) Integer.valueOf(data[6]).intValue();
                            position.setNumberOfSatellites(sats);
                            //#debug
                            Debug.out.println(System.currentTimeMillis() + " ms - [" + Thread.currentThread().getName() + "] LocationAPI.parseNMEA(): Number of Satellites = " + sats);
                        }
                    } else {
                        //#debug
                        Debug.out.println(System.currentTimeMillis() + " ms - [" + Thread.currentThread().getName() + "] LocationAPI.parseNMEA: GPS: no valid Position found!");
                    }
                } catch (Exception e) {
                    //#debug
                    Debug.out.println(System.currentTimeMillis() + " ms - [" + Thread.currentThread().getName() + "] LocationAPI.parseNMEA: <GGA>" + e.getMessage());
                }
            } else if (sLine.startsWith("$GPGSA")) {
                try {
                    String[] data = splitString(DATA_STRING);
//                    Debug.out.println( " DATA_STRING = " + DATA_STRING );
//                    Debug.out.println( "quality = " + data[1] + " HDOP = '" + data[15] + "' VDOP = '" + data[16] + "' " );
                    position.setQuality((byte) Integer.parseInt(data[1]));
                    //                Debug.out.println( "HDOP" );
                    try {
                        position.setHDOP(Float.parseFloat(data[15]));
                    } catch (NumberFormatException ex) {
                        position.setHDOP(Float.NaN);
                    }
                    //                Debug.out.println( "VDOP" );
                    try {
                        position.setVDOP(Float.parseFloat(data[16]));
                    } catch (NumberFormatException ex) {
                        position.setVDOP(Float.NaN);
                    }
                    //#debug
                    Debug.out.println(System.currentTimeMillis() + " ms - [" + Thread.currentThread().getName() + "] LocationAPI.parseNMEA(): $GPGSA contains: Quality = " + position.getQuality() + " - HDOP = " + position.getHDOP() + " - VDOP = " + position.getVDOP());
                } catch (Exception e) {
                    //#debug error
                    Debug.out.println(System.currentTimeMillis() + " ms - [" + Thread.currentThread().getName() + "] LocationAPI.parseNMEA(): <GSA>" + e.toString());
                }
            } else if ((position.getNumberOfSatellites() == GPSPosition.NUMBEROFSATELLITES_NOT_AVAILABLE) && sLine.startsWith("$GPGSV")) {
                try {
                    String[] data = splitString(DATA_STRING);
                    if (!data[2].equals("")) {
                        byte sats = (byte) Integer.valueOf(data[2]).intValue();
                        position.setNumberOfSatellites(sats);
                        //#debug
                        Debug.out.println(System.currentTimeMillis() + " ms - [" + Thread.currentThread().getName() + "] LocationAPI.parseNMEA(): GSV: Number of Satellites = " + sats);
                    }
                } catch (Exception e) {
                    //#debug error
                    Debug.out.println(System.currentTimeMillis() + " ms - [" + Thread.currentThread().getName() + "] LocationAPI.parseNMEA: <GSV>" + e.toString());
                }
            }
            nmea = nmea.substring(end_idx);
            begin_idx = nmea.indexOf('$');
            if (begin_idx > -1) {
                end_idx = nmea.indexOf("$", begin_idx + 1) - 1;
            } else {
                end_idx = -1;
            }
        /*            if ( timemiddle == 0 ) {
        timemiddle = (int)( System.currentTimeMillis() - currenttime );
        } else {
        timemiddle = ( timemiddle + (int)( System.currentTimeMillis() - currenttime ) ) / 2;
        }*/
        }
    }

    private void interpolate(GPSPosition pos, Orientation orient) {
        if (isInterpolated() && /*pos.getQuality() == GPSPosition.QUALITY_3D_FIX &&*/ !Float.isNaN(pos.getCourse()) && pos.getSpeed() > 5) {
            //#debug
            Debug.out.print(System.currentTimeMillis() + " ms - [" + Thread.currentThread().getName() + "] LocationAPI.interpolate(): Interpolate position");
            // convert speed from km/h to m/s
            float speed = pos.getSpeed() * 10.0F / 36.0F;

            double dx = speed * Math.sin(Math.toRadians(pos.getCourse()));
            double dy = speed * Math.cos(Math.toRadians(pos.getCourse()));
            //#debug
            Debug.out.println(" by " + dx + "/" + dy + " m");
            pos.movePositionMetre(dx, dy, 0);
            setCurrentPosition(pos, orient);
        } else {
            setCurrentPosition(pos, orient);
        }
    }
    /*
     * Split the gps data string and return a string array.
     * Example of GGA string: 123519,4807.038,N,01131.324,E,1,08,0.9,545.4,M,46.9,M, , *42
     */

    /**
     * @return
     * @param s
     */
    private String[] splitString(String s) throws Exception {
        int i = 0;
        int pos = 0;
        int nextPos = 0;
        // check how big the array is.
        while (pos > -1) {
            pos = s.indexOf(",", pos);
            if (pos < 0) {
                continue;
            }
            pos++;
            i++;
        }

        if (i > 25) {
            throw new Exception("too big:" + i);
        }
        i = 0;
        pos = 0;
        String[] result = new String[25];
        // Start splitting the string, search for each ','
        while (pos > -1) {
            pos = s.indexOf(",", pos);
            if (pos < 0) {
                continue;
            }
            nextPos = s.indexOf(",", pos + 1);
            if (nextPos < 0) {
                nextPos = s.length();
            }

            result[i] = s.substring(pos + 1, nextPos);
            i++;
            if (pos > -1) {
                pos++;
            }
        }

        return result;
    }

    /**
     * @param provider
     * @param newState
     */
    @Override
    public void providerStateChanged(LocationProvider provider, int newState) {
        //#debug
        Debug.out.println(System.currentTimeMillis() + " ms - [" + Thread.currentThread().getName() + "] LocationAPI.providerStateChanged(): provider : " + provider + " - providerStateChanged: " + newState);
        setState(newState);
        if (newState == LocationProvider.OUT_OF_SERVICE) {
            invokeDisconnected();
        }
    }
}
